/*
 * Errors.java
 *
 * Created on 25 de noviembre de 2007, 11:16 PM
 *
 * To change this template, choose Tools | Template Manager
 * and open the template in the editor.
 */

package Utils;
import Behaviors.Formations.FormationGenerator;
import Behaviors.NxPSDetectCentroid;
import Robot.Position2d;
import java.util.Locale;
/**
 *
 * @author jonathan
 */
public class Errors implements Runnable {
    private FormationGenerator formation;
    NxPSDetectCentroid detectCentroid=null;
    private Position2d [] positions;
    private Position2d center=null;
    private Vector2d poseRobot1=null;
    private Vector2d poseRobot2=null;
    private Vector2d poseRobot3=null;
    private Vector2d poseRobot4=null;
    private Vector2d centroid=null;
    
    private Vector2d poseInFormation1=null;
    private Vector2d poseInFormation2=null;
    private Vector2d poseInFormation3=null;
    private Vector2d poseInFormation4=null;
    
    private double error1=0.0f;
    private double error2=0.0;
    private double error3=0.0;
    private double error4=0.0;
    
    private long initialTime=0;
    private long time=0;
    
    private DataFile file=null;
    /** Creates a new instance of Errors */
    public Errors(FormationGenerator formation, NxPSDetectCentroid detectCentroid,Position2d center,Position2d [] positions) {
        this.formation=formation;
        this.detectCentroid=detectCentroid;
        this.positions=positions;
        this.center=center;
        this.initialTime=System.currentTimeMillis();
        file=new DataFile("errores","/home/alejo/");
        file.writeString("Tiempo\tError Robot1\tError Robot2\tError Robot3\tError Robot4\n");
        Locale.setDefault(new Locale("es","CO"));       
    }
    
    public void run(){
        while(true){
            centroid=detectCentroid.Value();
            Pose2d centerPosition = center.read(Position2d.READ_MODE_NO_WAIT);
            float centerAngle=(float)centerPosition.getPa();
            poseRobot1=positions[0].getPosition();
            poseRobot2=positions[1].getPosition();
            poseRobot3=positions[2].getPosition();
            poseRobot4=positions[3].getPosition();
            ///*
            poseInFormation1=formation.getPositionForRobot(positions[0].getRobotId()-1);
            poseInFormation2=formation.getPositionForRobot(positions[1].getRobotId()-1);
            poseInFormation3=formation.getPositionForRobot(positions[2].getRobotId()-1);
            poseInFormation4=formation.getPositionForRobot(positions[3].getRobotId()-1);
            //*/
            poseInFormation1=poseInFormation1.rotate(centerAngle);
            poseInFormation2=poseInFormation2.rotate(centerAngle);
            poseInFormation3=poseInFormation3.rotate(centerAngle);
            poseInFormation4=poseInFormation4.rotate(centerAngle);
            /*
            poseInFormation1=formation.getActualFormation().getRobotPosition(0);
            poseInFormation2=formation.getActualFormation().getRobotPosition(1);
            poseInFormation3=formation.getActualFormation().getRobotPosition(2);
            poseInFormation4=formation.getActualFormation().getRobotPosition(3);
            //*/
            
            poseInFormation1=poseInFormation1.add(centroid);
            poseInFormation2=poseInFormation2.add(centroid);
            poseInFormation3=poseInFormation3.add(centroid);
            poseInFormation4=poseInFormation4.add(centroid);
            //*/
            
            /*
            System.out.println("Position in formation 1: "+poseInFormation1.toString());
            System.out.println("Position in formation 2: "+poseInFormation2.toString());
            System.out.println("Position in formation 3: "+poseInFormation3.toString());
            System.out.println("Position in formation 4: "+poseInFormation4.toString());
            
            System.out.println("Position robot 1: "+poseRobot1.toString());
            System.out.println("Position robot 2: "+poseRobot2.toString());
            System.out.println("Position robot 3: "+poseRobot3.toString());
            System.out.println("Position robot 4: "+poseRobot4.toString());
                       
            //*/
                  
            error1=poseRobot1.distanceTo(poseInFormation1);
            error2=poseRobot2.distanceTo(poseInFormation2);
            error3=poseRobot3.distanceTo(poseInFormation3);
            error4=poseRobot4.distanceTo(poseInFormation4);
            
            time=System.currentTimeMillis()-initialTime;
            
            
            file.writeString(Long.toString(time)+"\t"+Double.toString(error1)+"\t"+Double.toString(error2)+"\t"+Double.toString(error3)+"\t"+Double.toString(error4)+"\n");
            
            
            
            try{Thread.sleep(500);}catch(Exception e){}
        }
    
    }    
}
